/*
 clustering.cpp
 Matthew Jee
 mcjee@ucsc.edu
*/

#include "clustering.h"
#include "debug.h"
#include <map>
#include <cmath>
#include <cassert>

#include <iostream>

namespace mj {

/*
    Path Clustering Functions
*/

static struct {
    size_t pathCount;
    double *distanceCache;
} _pathDistanceCache;

std::vector<Cluster> clusterPathsSimple(std::vector<Path> &paths, double size) {
    std::vector<Cluster> clusters;
    std::vector<Path>::iterator i;
    for (i = paths.begin(); i != paths.end(); ++i) {
        Path &path = *i;
        int index = i-paths.begin();
        Path::iterator j;
        Cluster cluster;
        cluster.tag = index;
        for (j = path.begin(); j != path.end(); ++j) {
            Point &point = *j;
            cluster.points.push_back(point);
        }
        clusters.push_back(cluster);
    }
    return clusters;
}

double pathDistance(Path &p, Path &q) {
    double distance = 0.0;
    size_t end = p.size() > q.size() ? p.size() : q.size();
    for (size_t i = 0; i < end; ++i) {
        Point &pPoint = p[i];
        Point &qPoint = q[i];
        distance += (pPoint.position-qPoint.position).length();
    }
    distance /= end;
    return distance;
}

double pathDistanceAlignFront(Path &p, Path &q) {
    // Find closest point on q to first point of p
    double distanceQToFirstP = INFINITY;
    Point &pFirst = p[0];
    Path::iterator closestQ;
    for (Path::iterator i = q.begin(); i != q.end(); ++i) {
        Point &qPoint = *i;
        double curDistance = (qPoint.position-pFirst.position).length();
        if (curDistance < distanceQToFirstP) {
            distanceQToFirstP = curDistance;
            closestQ = i;
        }
    }
    // Find closest point on p to first point of q
    double distancePToFirstQ = INFINITY;
    Point &qFirst = q[0];
    Path::iterator closestP;
    for (Path::iterator i = p.begin(); i != p.end(); ++i) {
        Point &pPoint = *i;
        double curDistance = (pPoint.position-qFirst.position).length();
        if (curDistance < distancePToFirstQ) {
            distancePToFirstQ = curDistance;
            closestP = i;
        }
    }

    // Do path distance calculation
    Path::iterator i;
    Path::iterator j;
    if (distancePToFirstQ < distanceQToFirstP) {
        i = closestP;
        j = q.begin();
    } else {
        i = p.begin();
        j = closestQ;
    }

    int count = 0;
    double distance = 0.0;
    for (; i != p.end() && j != q.end(); ++i, ++j) {
        Point &pPoint = *i;
        Point &qPoint = *j;
        distance += (pPoint.position-qPoint.position).length();
        count++;
    }
    size_t smaller = p.size() < q.size() ? p.size() : q.size();
    double penalty = 1.0*smaller/count;
    distance /= count;
    return distance*(penalty < 1.0 ? 1.0 : penalty*penalty);
}

double pathDistanceAlignBack(Path &p, Path &q) {
    // Find closest point on q to last point of p
    double distanceQToLastP = INFINITY;
    Point &pLast = p.back();
    Path::reverse_iterator closestQ;
    for (Path::reverse_iterator i = q.rbegin(); i != q.rend(); ++i) {
        Point &qPoint = *i;
        double curDistance = (qPoint.position-pLast.position).length();
        if (curDistance < distanceQToLastP) {
            distanceQToLastP = curDistance;
            closestQ = i;
        }
    }
    // Find closest point on p to last point of q
    double distancePToLastQ = INFINITY;
    Point &qLast = q.back();
    Path::reverse_iterator closestP;
    for (Path::reverse_iterator i = p.rbegin(); i != p.rend(); ++i) {
        Point &pPoint = *i;
        double curDistance = (pPoint.position-qLast.position).length();
        if (curDistance < distancePToLastQ) {
            distancePToLastQ = curDistance;
            closestP = i;
        }
    }
    // Do path distance calculation
    Path::reverse_iterator i;
    Path::reverse_iterator j;
    if (distancePToLastQ < distanceQToLastP) {
        i = closestP;
        j = q.rbegin();
    } else {
        i = p.rbegin();
        j = closestQ;
    }
    int count = 0;
    double distance = 0.0;
    for (; i != p.rend() && j != q.rend(); ++i, ++j) {
        Point &pPoint = *i;
        Point &qPoint = *j;
        distance += (pPoint.position-qPoint.position).length();
        count++;
    }
    distance /= count;
    return distance;
}

double pathDistanceAlignFrontCached(Path &p, Path &q) {
    assert(p.size() > 0);
    assert(q.size() > 0);
    size_t cacheIndex;
    size_t pID = p[0].pathID;
    size_t qID = q[0].pathID;
    if (pID < qID) {
        cacheIndex = pID*_pathDistanceCache.pathCount+qID;
    } else {
        cacheIndex = qID*_pathDistanceCache.pathCount+pID;
    }
    return _pathDistanceCache.distanceCache[cacheIndex];
}

void initPathDistanceCache(std::vector<Path> &paths) {
    assert(paths.size() > 0);
    // allocate distance array
    _pathDistanceCache.pathCount = paths.size();
    _pathDistanceCache.distanceCache = new double[paths.size()*paths.size()];
    // calculate all distances and stick them in the array
    std::vector<Path>::iterator i;
    std::vector<Path>::iterator j;
    for (i = paths.begin(); i != paths.end(); ++i) {
        std::cerr << 1.0-((float)(paths.end()-i)/paths.size()) << "%" << std::endl;
        size_t pIndex = i-paths.begin();
        assert((*i).size() > 0);
        for (j = i+1; j != paths.end(); ++j) {
            size_t qIndex = j-paths.begin();
            assert((*j).size() > 0);
            size_t cacheIndex = pIndex*paths.size()+qIndex;
            _pathDistanceCache.distanceCache[cacheIndex] = pathDistanceAlignFront(*i, *j);
        }
    }
}

void clearPathDistanceCache(void) {
    _pathDistanceCache.pathCount = 0;
    delete _pathDistanceCache.distanceCache;
}

// returns indices of points corresponding to points in the epsilon neighborhood of p
static std::vector<int> epsilonNeighborhoodPath(std::vector<Path> &paths,
                                                Path &p,
                                                double epsilon,
                                                double (*distanceFunction)(Path &, Path &)) {
    std::vector<int> neighborhood;
    for (size_t i = 0; i < paths.size(); ++i) {
        Path &q = paths[i];
        if (distanceFunction(p, q) <= epsilon) {
            neighborhood.push_back(i);
        }
    }
    return neighborhood;
}

std::vector<Cluster> clusterPathsDBSCAN(std::vector<Path> &paths,
                                        size_t minPaths,
                                        double epsilon,
                                        double (*distanceFunction)(Path &, Path &)) {
    std::vector<Cluster> clusters;

    // initialize
    size_t count = paths.size();
    bool *visited = new bool[count];
    int *cluster = new int[count];
    for (size_t i = 0; i < count; i++) {
        visited[i] = false;
        cluster[i] = -1; // no cluster assigned
    }
    int tag = 0;

    for (size_t i = 0; i < count; ++i) {
        Path &p = paths[i];
        std::cerr << ((float)i/count) << "%" << std::endl;
        if (!visited[i]) {
            visited[i] = true;
            std::vector<int> neighborhood = epsilonNeighborhoodPath(paths, p, epsilon, distanceFunction);
            if (neighborhood.size() < minPaths) {
                cluster[i] = -1;
            } else {

                clusters.push_back(Cluster());
                // expand cluster
                size_t clusterIndex = clusters.size()-1;
                cluster[i] = clusterIndex;
                clusters[clusterIndex].points.insert(clusters[clusterIndex].points.end(), p.begin(), p.end());
                clusters[clusterIndex].tag = tag++;
                for (size_t j = 0; j < neighborhood.size(); ++j) {
                    int q = neighborhood[j];
                    if (!visited[q]) {
                        visited[q] = true;
                        std::vector<int> qNeighborhood = epsilonNeighborhoodPath(paths, paths[q], epsilon, distanceFunction);
                        if (qNeighborhood.size() >= minPaths) {
                            neighborhood.insert(neighborhood.end(), qNeighborhood.begin(), qNeighborhood.end());
                        }
                    }
                    if (cluster[q] == -1) {
                        cluster[q] = clusterIndex;
                        clusters[clusterIndex].points.insert(clusters[clusterIndex].points.end(), paths[q].begin(), paths[q].end());
                    }
                }
            }
        }
    }
    delete visited;
    delete cluster;

    return clusters;
}

}
